//#include <pcl/console/print.h>
#include "ShowCollisionWorld\ShowCollisionWorld.h"
//#include "GlutDemoApplication.h"
//#include "LinearMath/btAlignedObjectArray.h"
#include <stdio.h>
#include <iostream>
#include "KinematicModel\Point.h"


int captureScreen();
int getMainCluster();
std::vector< object > nearestNeighbors(int cluster_no);




int main (int argc, char** argv) {

	int no;
	std::vector< object > objects;
	stringstream ss;
	std::vector<std::string> cloud_path;
	std::vector<Point> path;

    pcl::console::print_info ("Capturing camera data... \n"); 
	
	//take a screenshot
	//captureScreen();
	
	//clusters it
	int x = getMainCluster();
	//find NN
	objects = nearestNeighbors(x);

	printf("\n\n\nInsert the number of the cluster representing the object you would like to pick up: ");
	scanf("%d",&no);	
  

	int i;
	for(i = 0; i < objects.size(); i++){
		
		ss << "cloud_cluster_" << objects.at(i).clusterNo << ".pcd";
		cloud_path.push_back(ss.str());
		ss.str("");

	}

	ShowCollisionWorld SCW(cloud_path, 0.005, argc, argv, objects, no);
	path = SCW.getPath();


	std::cout << "CLASS: " << path.size() << std::endl;

	pcl::console::print_info ("\nDone!");
	/*char key;
	std::cin >> key;*/
	return 1;
}